#include "inc/hw_types.h"		// tBoolean
#include "inc/hw_memmap.h"
#include "RASLib/init.h"
#include "driverlib/gpio.h"
#include "driverlib/sysctl.h"

#include "RASLib/motor.h"
#include "RASLib/encoder.h"
#include "RASLib/servo.h"

void testTheEncoders(void);
void testTheServoAndIR(void);
void initMotors(void);
void initLineSensor(void);
void initUART(void);
void initIRSensor(void);
void initEncoders(void);
void Wait(long ms);
void initServo(void);
void SetDischargeTime(int ms);
long getADCValue(void);
unsigned char ReadLineSensor(void);
void UARTprintf(const char *pcString, ...);

void testAllThings(){
	testTheEncoders();
}

void testTheEncoders(){				 
	signed long encoder0, encoder1;
	initEncoders();
	initMotors();
	PresetEncoderCounts(0, 0);
	SetMotorPowers(127,127-23);
	Wait(5000);
	
	while(true) {	  
		GetEncoderCounts(&encoder0, &encoder1);
		UARTprintf("%d %d\r",encoder0, encoder1);
		break;
	}
	SetMotorPowers(0,0);
}

void testTheServoAndIR(){
	unsigned char position=0;
	int direction=1;
	initServo();
	initIRSensor();

	while(true){
		unsigned long ADCValue = getADCValue();
		//SetServoPosition(SERVO_0, position);
		position+=direction;
		if(position>=255||position==0)direction*=-1;
		Wait(5);
		UARTprintf("%d   \r",ADCValue);
	}
}
